/*******************************************************************************
 * Copyright (c) 2014, 2015 IBM Corp.
 *
 * All rights reserved. This program and the accompanying materials
 * are made available under the terms of the Eclipse Public License v1.0
 * and Eclipse Distribution License v1.0 which accompany this distribution.
 *
 * The Eclipse Public License is available at
 *    http://www.eclipse.org/legal/epl-v10.html
 * and the Eclipse Distribution License is available at
 *   http://www.eclipse.org/org/documents/edl-v10.php.
 *
 * Contributors:
 *    Allan Stockdill-Mander/Ian Craggs - initial API and implementation and/or initial documentation
 *******************************************************************************/
#include "MQTTClient.h"

static const char* MQTTPacket_names[] = {
	"RESERVED", "CONNECT", "CONNACK", "PUBLISH", "PUBACK", "PUBREC", "PUBREL",
	"PUBCOMP", "SUBSCRIBE", "SUBACK", "UNSUBSCRIBE", "UNSUBACK",
	"PINGREQ", "PINGRESP", "DISCONNECT"
};

static void NewMessageData(MessageData* md, MQTTString* aTopicName, MQTTMessage* aMessage)
{
	md->topicName = aTopicName;
	md->message = aMessage;
}


static int getNextPacketId(MQTTClient *c)
{
	return c->next_packetid = (c->next_packetid == MAX_PACKET_ID) ? 1 : c->next_packetid + 1;
}


static int sendPacket(MQTTClient* c, int length, Timer* timer)
{
	int rc = FAILURE, sent = 0;

	while ((sent < length) && !TimerIsExpired(timer)) {
		rc = c->ipstack->mqttwrite(c->ipstack, &c->buf[sent], length, TimerLeftMS(timer));
		if (rc < 0)
			break;
		sent += rc;
	}
	/*
		The send progress only send the packet to the socket buffer
		we dont know if it successeds, default it is SUCCESS
	*/

	if (sent == length) {		
		rc = SUCCESS;
	} else {
		rc = FAILURE;
	}

	return rc;
}


void MQTTClientDeinit(MQTTClient* c)
{
#if defined(MQTT_TASK)
	FreeRTOS_MutexDeinit(&c->mutex);
#endif
}


void MQTTClientInit(MQTTClient* c, Network* network, unsigned int command_timeout_ms,
                    unsigned char* sendbuf, size_t sendbuf_size, unsigned char* readbuf, size_t readbuf_size)
{
	int i;
	c->ipstack = network;

	for (i = 0; i < MAX_MESSAGE_HANDLERS; ++i)
		c->messageHandlers[i].topicFilter = 0;
	c->command_timeout_ms = command_timeout_ms;
	c->buf = sendbuf;
	c->buf_size = sendbuf_size;
	c->readbuf = readbuf;
	c->readbuf_size = readbuf_size;
	c->isconnected = 0;
	c->ping_outstanding = 0;
	c->defaultMessageHandler = NULL;
	c->next_packetid = 1;
	TimerInit(&c->ping_timer);
#if defined(MQTT_TASK)
	FreeRTOS_MutexInit(&c->mutex);
#endif
}


static int decodePacket(MQTTClient* c, int* value, int timeout)
{
	unsigned char i;
	int multiplier = 1;
	int len = 0;
	const int MAX_NO_OF_REMAINING_LENGTH_BYTES = 4;

	*value = 0;
	do {
		int rc = MQTTPACKET_READ_ERROR;

		if (++len > MAX_NO_OF_REMAINING_LENGTH_BYTES) {
			rc = MQTTPACKET_READ_ERROR; /* bad data */
			goto exit;
		}
		rc = c->ipstack->mqttread(c->ipstack, &i, 1, timeout);
		if (rc != 1)
			goto exit;
		*value += (i & 127) * multiplier;
		multiplier *= 128;
	} while ((i & 128) != 0);
exit:
	return len;
}

#define print serial_printf

static int readPacket(MQTTClient* c, Timer* timer)
{
	int rc = FAILURE;
	MQTTHeader header = {0};
	int len = 0;
	int rem_len = 0;

#ifdef CONFIG_AXTLS
	if(!(len = c->ipstack->mqttread(c->ipstack, c->readbuf, c->readbuf_size, TimerLeftMS(timer))))
		goto exit;
#else
	/* 1. read the header byte.  This has the packet type in it */
	if (c->ipstack->mqttread(c->ipstack, c->readbuf, 1, TimerLeftMS(timer)) != 1)
		goto exit;

	len = 1;
	/* 2. read the remaining length.  This is variable in itself */
	decodePacket(c, &rem_len, TimerLeftMS(timer));

	len += MQTTPacket_encode(c->readbuf + 1, rem_len); /* put the original remaining length back into the buffer */

	/* 3. read the rest of the buffer using a callback to supply the rest of the data */
	if (rem_len > 0 && (c->ipstack->mqttread(c->ipstack, c->readbuf + len, rem_len, TimerLeftMS(timer)) != rem_len))
		goto exit;
#endif

	header.byte = c->readbuf[0];
	rc = header.bits.type;
exit:
	return rc;
}


// assume topic filter and name is in correct format
// # can only be at end
// + and # can only be next to separator
static char isTopicMatched(char* topicFilter, MQTTString* topicName)
{
	char* curf = topicFilter;
	char* curn = topicName->lenstring.data;
	char* curn_end = curn + topicName->lenstring.len;

	while (*curf && curn < curn_end) {
		if (*curn == '/' && *curf != '/')
			break;
		if (*curf != '+' && *curf != '#' && *curf != *curn)
			break;
		if (*curf == '+') {
			// skip until we meet the next separator, or end of string
			char* nextpos = curn + 1;
			while (nextpos < curn_end && *nextpos != '/')
				nextpos = ++curn + 1;
		} else if (*curf == '#')
			curn = curn_end - 1;    // skip until end of string
		curf++;
		curn++;
	};

	return (curn == curn_end) && (*curf == '\0');
}


int deliverMessage(MQTTClient* c, MQTTString* topicName, MQTTMessage* message)
{
	int i;
	int rc = FAILURE;

	// we have to find the right message handler - indexed by topic
	for (i = 0; i < MAX_MESSAGE_HANDLERS; ++i) {
		if (c->messageHandlers[i].topicFilter != 0 && (MQTTPacket_equals(topicName, (char*)c->messageHandlers[i].topicFilter) ||
		        isTopicMatched((char*)c->messageHandlers[i].topicFilter, topicName))) {
			if (c->messageHandlers[i].fp != NULL) {
				MessageData md;
				NewMessageData(&md, topicName, message);
				c->messageHandlers[i].fp(&md);
				rc = SUCCESS;
			}
		}
	}

	if (rc == FAILURE && c->defaultMessageHandler != NULL) {
		MessageData md;
		NewMessageData(&md, topicName, message);
		c->defaultMessageHandler(&md);
		rc = SUCCESS;
	}

	return rc;
}


int keepalive(MQTTClient* c)
{
	int rc = SUCCESS;

	if (c->keepAliveInterval == 0) {
		rc = SUCCESS;
		goto exit;
	}

	if (TimerIsExpired(&c->ping_timer)) {
		printf("keep alive counter expired\n");
		if (!c->ping_outstanding) {
			printf("keep alive counter reset\n");
			//Set the pingreq send timeout
			Timer timer;
			TimerInit(&timer);
			TimerCountdownMS(&timer, 1000);
			int len = MQTTSerialize_pingreq(c->buf, c->buf_size);
			/* If the sendPacket success, the keep alive works with keepAliveInterval */
			if (len > 0 && (rc = sendPacket(c, len, &timer)) == SUCCESS)  {// send the ping packet
				c->ping_outstanding = 1;
				TimerCountdown(&c->ping_timer, c->keepAliveInterval); // record the fact that we have successfully sent the packet
			} else {
				rc = FAILURE;
			}
		} else {
		
			printf("keep alive counter failure\n");

			rc = FAILURE;
		}
	}

exit:
	return rc;
}

#define print serial_printf
#define printf serial_printf

/*


	success -> 
  		return packet type
  	fail->
		return FAIL

*/
int cycle(MQTTClient* c, Timer* timer)
{
	int packet_type = readPacket(c, timer);
	int len = 0;
	int rc = SUCCESS;

	if (FAILURE == packet_type) {
		packet_type = NOPACKET;
		goto exit;
	}
	printf("received %s\n", MQTTPacket_names[packet_type]);

	switch (packet_type)
	{
		case CONNACK:
		case PUBACK:
		case SUBACK:
			break;
		case PUBLISH:
		{
			MQTTString topicName;
			MQTTMessage msg;
			int intQoS;
			if (MQTTDeserialize_publish(&msg.dup, &intQoS, &msg.retained, &msg.id, &topicName,
					(unsigned char**)&msg.payload, (int*)&msg.payloadlen, c->readbuf, c->readbuf_size) != 1)
				goto exit;

			msg.qos = (enum QoS)intQoS;
			//deliverMessage(c, &topicName, &msg);

			if (msg.qos != QOS0) {
				if (msg.qos == QOS1)
					len = MQTTSerialize_ack(c->buf, c->buf_size, PUBACK, 0, msg.id);
				else if (msg.qos == QOS2)
					len = MQTTSerialize_ack(c->buf, c->buf_size, PUBREC, 0, msg.id);

				if (len <= 0)
					rc = FAILURE;
				else
					rc = sendPacket(c, len, timer);

				if (rc == FAILURE)
					goto exit; // there was a problem
			}
			deliverMessage(c, &topicName, &msg);

			break;
		}
		case PUBREC:
		{
			unsigned short mypacketid;
			unsigned char dup, type;
			if (MQTTDeserialize_ack(&type, &dup, &mypacketid, c->readbuf, c->readbuf_size) != 1)
				rc = FAILURE;
			else if ((len = MQTTSerialize_ack(c->buf, c->buf_size, PUBREL, 0, mypacketid)) <= 0)
				rc = FAILURE;
			else if ((rc = sendPacket(c, len, timer)) != SUCCESS) // send the PUBREL packet
				rc = FAILURE; // there was a problem
			if (rc == FAILURE)
				goto exit; // there was a problem
			break;
		}
		case PUBCOMP:
			break;
		case PINGRESP:
			c->ping_outstanding = 0;
			//c->keepAliveFailCount = 0;
			break;
		case PUBREL:
		{
			unsigned short mypacketid;
			unsigned char dup, type;
			if (MQTTDeserialize_ack(&type, &dup, &mypacketid, c->readbuf, c->readbuf_size) != 1)
				rc = FAILURE;
			else if ((len = MQTTSerialize_ack(c->buf, c->buf_size, PUBCOMP, 0, mypacketid)) <= 0)
				rc = FAILURE;
			else if ((rc = sendPacket(c, len, timer)) != SUCCESS) // send the PUBCOMP packet
				rc = FAILURE; // there was a problem
			if (rc == FAILURE)
				goto exit; // there was a problem
			break;
		}
	}

exit:

	if (FAILURE == keepalive(c)) {
		rc = FAILURE;
	}

	if (rc == SUCCESS)
		rc = packet_type;

	return rc;
}


int MQTTYield(MQTTClient* c, int timeout_ms)
{
	int rc = SUCCESS;
	Timer timer;
	TimerInit(&timer);

#if defined(MQTT_TASK)
	FreeRTOS_MutexLock(&c->mutex);
#endif
	TimerCountdownMS(&timer, timeout_ms);
	//printf(".");
	rc = cycle(c, &timer);
#if defined(MQTT_TASK)
	FreeRTOS_MutexUnlock(&c->mutex);
#endif
	sys_msleep(1); 

	return rc;
}


void MQTTRun(void* parm)
{
	Timer timer;
	MQTTClient* c = (MQTTClient*)parm;

	TimerInit(&timer);

	while (1) {
#if defined(MQTT_TASK)
		FreeRTOS_MutexLock(&c->mutex);
#endif
		TimerCountdownMS(&timer, 500); /* Don't wait too long if no traffic is incoming */
		cycle(c, &timer);
#if defined(MQTT_TASK)
		FreeRTOS_MutexUnlock(&c->mutex);
#endif
	}
}


#if defined(MQTT_TASK)
int MQTTStartTask(MQTTClient* client)
{
	return ThreadStart(&client->thread, &MQTTRun, client);
}
#endif


int waitfor(MQTTClient* c, int packet_type, Timer* timer)
{
	int rc = FAILURE;

	do {
		if (TimerIsExpired(timer))
			break; // we timed out
	} while ((rc = cycle(c, timer)) != packet_type);

	return rc;
}


int MQTTConnect(MQTTClient* c, MQTTPacket_connectData* options)
{
	Timer connect_timer;
	int rc = FAILURE;
	MQTTPacket_connectData default_options = MQTTPacket_connectData_initializer;
	int len = 0;

#if defined(MQTT_TASK)
	FreeRTOS_MutexLock(&c->mutex);
#endif
	if (c->isconnected) /* don't send connect packet again if we are already connected */
		goto exit;

	TimerInit(&connect_timer);
	TimerCountdownMS(&connect_timer, c->command_timeout_ms);

	if (options == 0)
		options = &default_options; /* set default options if none were supplied */

	c->keepAliveInterval = options->keepAliveInterval;
	TimerCountdown(&c->ping_timer, c->keepAliveInterval);
	if ((len = MQTTSerialize_connect(c->buf, c->buf_size, options)) <= 0)
		goto exit;
	if ((rc = sendPacket(c, len, &connect_timer)) != SUCCESS) 
		goto exit;
	// this will be a blocking call, wait for the connack
	if (waitfor(c, CONNACK, &connect_timer) == CONNACK) {
		unsigned char connack_rc = 255;
		unsigned char sessionPresent = 0;
		if (MQTTDeserialize_connack(&sessionPresent, &connack_rc, c->readbuf, c->readbuf_size) == 1)
			rc = connack_rc;
		else
			rc = FAILURE;
	} else
		rc = FAILURE;

exit:
	if (rc == SUCCESS)
		c->isconnected = 1;

#if defined(MQTT_TASK)
	FreeRTOS_MutexUnlock(&c->mutex);
#endif

	return rc;
}


int MQTTSubscribe(MQTTClient* c, const char* topicFilter, enum QoS qos, messageHandler messageHandler)
{
	int rc = FAILURE;
	Timer timer;
	int len = 0;
	MQTTString topic = MQTTString_initializer;
	topic.cstring = (char *)topicFilter;

#if defined(MQTT_TASK)
	FreeRTOS_MutexLock(&c->mutex);
#endif
	if (!c->isconnected)
		goto exit;

	TimerInit(&timer);
	TimerCountdownMS(&timer, c->command_timeout_ms);

	len = MQTTSerialize_subscribe(c->buf, c->buf_size, 0, getNextPacketId(c), 1, &topic, (int*)&qos);
	if (len <= 0)
		goto exit;
	if ((rc = sendPacket(c, len, &timer)) != SUCCESS) // send the subscribe packet
		goto exit;             // there was a problem

	if (waitfor(c, SUBACK, &timer) == SUBACK) {    // wait for suback
		int count = 0, grantedQoS = -1;
		unsigned short mypacketid;
		if (MQTTDeserialize_suback(&mypacketid, 1, &count, &grantedQoS, c->readbuf, c->readbuf_size) == 1)
			rc = grantedQoS; // 0, 1, 2 or 0x80
		if (rc != 0x80) {
			int i;
			for (i = 0; i < MAX_MESSAGE_HANDLERS; ++i) {
				if (c->messageHandlers[i].topicFilter == 0) {
					c->messageHandlers[i].topicFilter = topicFilter;
					c->messageHandlers[i].fp = messageHandler;
					rc = 0;
					break;
				}
			}
		}
	} else
		rc = FAILURE;

exit:
#if defined(MQTT_TASK)
	FreeRTOS_MutexUnlock(&c->mutex);
#endif
	return rc;
}


int MQTTUnsubscribe(MQTTClient* c, const char* topicFilter)
{
	int rc = FAILURE;
	Timer timer;
	MQTTString topic = MQTTString_initializer;
	topic.cstring = (char *)topicFilter;
	int len = 0;

#if defined(MQTT_TASK)
	FreeRTOS_MutexLock(&c->mutex);
#endif
	if (!c->isconnected)
		goto exit;

	TimerInit(&timer);
	TimerCountdownMS(&timer, c->command_timeout_ms);

	if ((len = MQTTSerialize_unsubscribe(c->buf, c->buf_size, 0, getNextPacketId(c), 1, &topic)) <= 0)
		goto exit;
	if ((rc = sendPacket(c, len, &timer)) != SUCCESS) // send the subscribe packet
		goto exit; // there was a problem

	if (waitfor(c, UNSUBACK, &timer) == UNSUBACK) {
		unsigned short mypacketid;  // should be the same as the packetid above
		if (MQTTDeserialize_unsuback(&mypacketid, c->readbuf, c->readbuf_size) == 1)
			rc = 0;
	} else
		rc = FAILURE;

exit:
#if defined(MQTT_TASK)
	FreeRTOS_MutexUnlock(&c->mutex);
#endif
	return rc;
}


int MQTTPublish(MQTTClient* c, const char* topicName, MQTTMessage* message)
{
	int rc = FAILURE;
	Timer timer;
	MQTTString topic = MQTTString_initializer;
	topic.cstring = (char *)topicName;
	int len = 0;

#if defined(MQTT_TASK)
	FreeRTOS_MutexLock(&c->mutex);
#endif
	if (!c->isconnected)
		goto exit;

	TimerInit(&timer);
	TimerCountdownMS(&timer, c->command_timeout_ms);

	if (message->qos == QOS1 || message->qos == QOS2)
		message->id = getNextPacketId(c);

	len = MQTTSerialize_publish(c->buf, c->buf_size, 0, message->qos, message->retained, message->id,
	                            topic, (unsigned char*)message->payload, message->payloadlen);
	if (len <= 0)
		goto exit;
	if ((rc = sendPacket(c, len, &timer)) != SUCCESS) // send the subscribe packet
		goto exit; // there was a problem

	if (message->qos == QOS1) {
		if (waitfor(c, PUBACK, &timer) == PUBACK) {
			unsigned short mypacketid;
			unsigned char dup, type;
			if (MQTTDeserialize_ack(&type, &dup, &mypacketid, c->readbuf, c->readbuf_size) != 1)
				rc = FAILURE;
		} else
			rc = FAILURE;
	} else if (message->qos == QOS2) {
		if (waitfor(c, PUBCOMP, &timer) == PUBCOMP) {
			unsigned short mypacketid;
			unsigned char dup, type;
			if (MQTTDeserialize_ack(&type, &dup, &mypacketid, c->readbuf, c->readbuf_size) != 1)
				rc = FAILURE;
		} else
			rc = FAILURE;
	}

exit:
#if defined(MQTT_TASK)
	FreeRTOS_MutexUnlock(&c->mutex);
#endif
	return rc;
}


int MQTTDisconnect(MQTTClient* c)
{
	int rc = FAILURE;
	Timer timer;     // we might wait for incomplete incoming publishes to complete
	int len = 0;

#if defined(MQTT_TASK)
	FreeRTOS_MutexLock(&c->mutex);
#endif
#if 1
	TimerInit(&timer);
	TimerCountdownMS(&timer, c->command_timeout_ms);

	len = MQTTSerialize_disconnect(c->buf, c->buf_size);
	if (len > 0)
		rc = sendPacket(c, len, &timer);            // send the disconnect packet
#endif
	c->isconnected = 0;

#if defined(MQTT_TASK)
	FreeRTOS_MutexUnlock(&c->mutex);
#endif
	return rc;
}

